/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: imu.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <math.h>
#include <pthread.h>

#include "serial.h"
#include "globaldefs.h"
#include "utilities.h"
#include "imu.h"

void imuRequest(int sPort)
{
	int 		nbytes=0;
	static		byte		packet2=RotAcelT;


  /*********************************************************************
   *Send DELTAS Command to IMU
   *********************************************************************/
  //nbytes = write(sPort2,(char*)&packet1, 1);
  /*********************************************************************
   *Read DELTAS
   *********************************************************************/
  //nanosleep(&t_DELTA,NULL);
  //nbytes = tcischars(sPort0);
  //printf("Tcischar DELTAS function %d\n",nbytes);
  //nbytes=read(sPort2,input_DELTAS,DELTAS_PACKET_LENGTH);
  //printf(" Read DELTAS Function %d\n",nbytes);
  /*********************************************************************
   *Send RotAcelT Command to IMU
  *********************************************************************/
  nbytes = write(sPort,(char*)&packet2, 1);
}  
  void imuAcquisition(int sPort, struct imu *imupacket)
{
  int		nbytes;	  
  byte  	input_DELTAS[DELTAS_PACKET_LENGTH]={0,};
  byte  	input_RotAcelT[RotAcelT_PACKET_LENGTH]={0,};

  /*********************************************************************
   *Read RotAcelT
  *********************************************************************/
  //nanosleep(&t_RotAcelT,NULL);
  //nbytes = tcischars(sPort0);
  //printf("Tcischar RotAcelT function %d\n",nbytes);
  nbytes=read(sPort,input_RotAcelT,67);
  //printf("Read RotAcelT Function %d\n",nbytes);
  
  /***************************************************************************************
    *Decode Deltas packet
  ***************************************************************************************/
  // if(checksumCheck(&input_DELTAS[0],DELTAS_PACKET_LENGTH)==TRUE)
   		  {
	   /*
   		  	//printf("[imu]:Nice,it worked deltas..!\n");
 	  		//delta velocity's in m/s
 	  		imupacket->dthetax=floatFromBytesBE(&input_DELTAS[1]); 
 	  		imupacket->dthetay=floatFromBytesBE(&input_DELTAS[5]); 
 	  		imupacket->dthetaz=floatFromBytesBE(&input_DELTAS[9]);
 			// delat thetas in rad
 	  		imupacket->dvx=floatFromBytesBE(&input_DELTAS[13]);
 	  		imupacket->dvy=floatFromBytesBE(&input_DELTAS[17]);
 	  		imupacket->dvz=floatFromBytesBE(&input_DELTAS[21]);
 	  		imupacket->timer_imu=longFromBytesBE(&input_DELTAS[25]);
   		 */  }
  // 		  else 
  //               	{
                 		//imupacket.error_type = CHECKSUM_ERROR;
   			//printf("Deltas CheckSum problem.!\n");
  //               	}
 
  /**************************************** ***********************************************
     *Decode RotAcelT packet
  ***************************************************************************************/
  if(nbytes>0)
  {
  if(checksumCheck(input_RotAcelT,RotAcelT_PACKET_LENGTH)==TRUE)
  		  {
  		  	//printf("[imu]:Nice,it worked deltas..!\n");
	  		//Accel
	  		//imupacket->ax=floatFromBytesBE(&input_RotAcelT[1]); 
	  		//imupacket->ay=floatFromBytesBE(&input_RotAcelT[5]); 
	  		//imupacket->az=floatFromBytesBE(&input_RotAcelT[9]);
			// AngRate
	  		//imupacket->p=floatFromBytesBE(&input_RotAcelT[13]); 
	  		//imupacket->q=floatFromBytesBE(&input_RotAcelT[17]); 
	  		//imupacket->r=floatFromBytesBE(&input_RotAcelT[21]);
			// Matrix
	  		imupacket->T11=floatFromBytesBE(&input_RotAcelT[25]); 
	  		imupacket->T12=floatFromBytesBE(&input_RotAcelT[29]); 
	  		imupacket->T13=floatFromBytesBE(&input_RotAcelT[33]);	  		
	  		//imupacket->T21=floatFromBytesBE(&input_RotAcelT[37]); 
	  		//imupacket->T22=floatFromBytesBE(&input_RotAcelT[41]); 
	  		imupacket->T23=floatFromBytesBE(&input_RotAcelT[45]);
	  		//imupacket->T31=floatFromBytesBE(&input_RotAcelT[49]); 
	  		//imupacket->T32=floatFromBytesBE(&input_RotAcelT[53]); 
	  		imupacket->T33=floatFromBytesBE(&input_RotAcelT[57]);	
	  		imupacket->pitch=-asinf(imupacket->T13);
	  		imupacket->roll=atan2f(imupacket->T23,imupacket->T33);
	  		imupacket->yaw=atan2f(imupacket->T12,imupacket->T11);
	  		//printf("IMU %f\n",imupacket->pitch);
  		  }
  		  else 
                	{
  			//printf("RotAcelT CheckSum problem.!\n");
                		//imupacket.error_type = CHECKSUM_ERROR; 
                	}
  }
    
} /* end void *imu_acq() */

